// -*- coding: utf-8 -*-

/**
 * A typical use of HCSLib.
 */

#include <iostream>
#include <string>

#include "elisa/hybrid/hybrid.h"

using namespace std;
using namespace elisa;

// Constants
const VNInterval G(VNInterval(98)/10);
const VNInterval K(VNInterval(3)/10);

// Definition of continuous constraint.
template<typename T> 
void dfunc(int n, T *dv, const T *v, T t, void *param)
{
	/*
	dv[0] = v[1];
	dv[1] = -G - K*v[1];
	*/
	dv[0] = v[2];
	dv[1] = v[3];
	dv[2] = T(0.0);
	//dv[3] = -G - K*v[3];
	dv[3] = -G + K*sqr(v[3]);
}

// Definition of guard constraint.
template<typename T> 
T gfunc(const std::vector<T>& v, const T& t)
{
	//return v[0];
	return v[1] - sin(2*v[0]);
}


int main()
{
	// initialize Elisa.
	elisa::init();
	RealVar::SetDigits(16);

	elisa::useBC5();
	//elisa::useHC4();
	//elisa::useHybrid();
	elisa::useHybridNewton(true);

	// Parameters of solving process.
	RealVar::SetPrecision(1.0e-1);
	Time::setPrecision(1.0e-1);

	// Variables.
	/*
	ContVar py(VNInterval(14,15)/10, "py");
	ContVar vy(-VNInterval(27)/10, "vy");

	ContVar px(1.0, "px");
	ContVar py(VNInterval(3.0,3.1), "py");
	ContVar vx(6.0, "vx");
	ContVar vy(-2.0, "vy");
	*/
	ContVar px(0.0, "px");
	ContVar py(VNInterval(14,15)/10, "py");
	ContVar vx(VNInterval(53)/10,  "vx");
	ContVar vy(-VNInterval(27)/10, "vy");

	ContVarVec cvv;
	cvv.push_back(&px);
	cvv.push_back(&py);
	cvv.push_back(&vx);
	cvv.push_back(&vy);

	std::vector<RealExpr> rev;
	rev.push_back(px);
	rev.push_back(py);
	rev.push_back(vx);
	rev.push_back(vy);

	Time t(v_bias::interval(0.0), cvv);
	t.setDomain(new Interval(0.0, 1.0e1));


	// Continuous constraint.
	ContConstraint cc(new ContConstraintRep(cvv, t, dfunc, dfunc),
		gfunc<VNInterval>, gfunc<AD_VNInterval>, dfunc<VNInterval>);

	// Guard constraint.
	RealConstraint gc(gfunc<RealExpr>(rev, t) == 0);

	// The model.
	Model model(cc && gc);
	
	// Instantiate the solver.
	HybridSolver solver(model, cc);
	//IntervalSolver solver(model);

	// Solving process.
	try {
		while (solver.next())
		{
			cout << "solution " << solver.numSolutions() << endl;

			cout << "\tt in " << t.domain() << endl;
			cout << '\t' << px.getName() << " in " << px.domain() << endl;
			cout << '\t' << py.getName() << " in " << py.domain() << endl;
			cout << '\t' << vx.getName() << " in " << vx.domain() << endl;
			cout << '\t' << vy.getName() << " in " << vy.domain() << endl;
		}
	} catch (const string& msg) {
		cout << msg << endl;
	}

	elisa::dismiss();

	return 0;
}
